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Abstract 

A  configuration  of  an  articulated  figure  of  joints  and  segments  can  sometimes  be  specified  as 
spatial  constraints.  Constrained  parts  on  the  articulated  figure  are  abstracted  as  end  effectors, 
and  the  counterparts  in  the  space  are  abstracted  as  goals.  The  goal  (constraint)  can  be  as  simple 
as  a  position,  an  orientation,  a  weighted  combination  of  position  and  orientation,  a  line,  a  plane, 
a  direction,  and  so  on,  or  it  could  be  as  complicated  as  a  region  in  the  space.  An  articulated 
figure  consists  of  various  segments  connected  together  by  joints.  Each  joint  has  some  degrees 
of  freedom  which  are  subject  to  joint  limits  and  manual  adjustment.  This  paper  presents  an 
efficient  algorithm  to  adjust  the  joint  angles  subject  to  joint  limits  so  that  the  set  of  end  effectors 
concurrently  attempt  to  achieve  their  respective  goals.  Users  specif  end  effectors  and  goals: 
the  program  computes  a  final  configuration  in  real  time  in  the  sense  tha*  ions  appear  to  take 
no  longer  than  actual  physical  activities  would.  If  it  is  impossible  to  satisfy  all  the  goals  owing 
to  the  actual  constraints,  the  program  should  end  up  with  the  best  possibility  according  to  the 
users’  assignment  of  importances  to  each  goal. 


1  Introduction 

The  ultimate  objective  of  computer  animation  is  to  create  various  motions  using  computers.  Often, 
we  are  given  motions  or  soin**  particular  points  in  ‘he  figure,  and  try  to  solve  the  whole 

motion.  The  specifications  on  those  particular  points  are  usually  called  constraints. 


1 


Badier  et  al  introduced  position  constraints  [1].  They  recursively  solved  for  joint  angles  of 
articulated  figures  to  satisfy  multiple  pcsition  constraints.  But  in  that  paper,  orientation  constraints 
and  joint  limits  were  not  dealt  with.  Moreover,  the  sequential  nature  of  the  tree  traversal  often  led 
to  realizable  but  awkward  solutions. 

Girard  and  Maciejewski  used  pseudo-inverse  of  Jacobian  matrix  to  solve  spatial  constraints  [8]. 
The  main  formula  is 

A9  =  J+Ar 

where  A 9  is  the  increment  of  the  joint  angle  vector,  Ar  is  the  increment  of  the  spatial  vector  and 
J+  is  the  pseudo-inverse  of  the  Jacobian  dr/dO.  If  we  use  a  large  step  size,  the  method  is  actually 
the  well  known  Newton-Raphson  method,  which  is  not  globally  convergent  and  often  needs  some 
special  handling  (e.  g.  hybrid  method  [10]).  Or  we  may  use  a  sufficiently  small  step  size,  which 
was  suggested  by  Girard  and  Maciejewski  in  [8];  but  this  requires  excessive  iterations.  The  inverse 
operation  is  usually  very  expensive  (say,  0(n3));  and  they  did  not  deal  with  joint  limits. 

Witkin  et  al  used  energy  constraints  [16].  The  energy  function  is  the  sum  of  all  constraints 
including  position  and  orientation  constraints.  Constraints  are  satisfied  if  and  only  if  the  energy 
function  ic  zero.  Their  method  is  to  find  the  integration  of  the  differential  equation: 

d9(t)/dt  =  -VE{9) 

where  0  is  the  parameter  vector,  E  is  the  energy  function  of  9,  and  V  is  the  gradient  operator. 
The  motion  is  actually  driven  by  the  conservative  force  which  serves  as  the  constraint  force  derived 
from  VE,  and  is  smooth  in  the  sense  that  the  parameter  vector  9  is  a  smooth  function  of  time  t. 
The  energy  function  may  incorporate  the  constraints  on  parameter  space  (joint  angle  space),  but 
it  treats  the  parameter  limits  (joint  limits)  as  penalty  functions  rather  than  directly.  Although  the 
penalty  function  method  is  very  effective  in  dealing  with  general  constraints,  we  have  a  much  more 
efficient  method  to  deal  with  linear  constraints 

Barzel  and  Barr  used  dynamic  constraints  to  solve  for  the  motion  [2].  In  their  approach, 
deviation  functions  were  introduced  such  that  the  constraints  are  met  if  and  only  if  the  deviation 
func' >n  is  zero.  They  solved  for  constraint  forces  such  that  the  deviation  function  decreases 
exponentially  in  terms  of  time  t  under  external  forces  and  constraint  forces,  i  his  method  gives 
very  attractive  motion  because  it  considers  not  only  constraint  forces  but  also  external  forces  such 
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as  gravity.  The  constraint  force  is  not  necessarily  a  conservative  force;  actually  it  is  derived  from  the 
deviation  function.  Joints  can  be  accomplished  by  point-to-point  constraints  with  their  approach. 
But  they  did  not  consider  joint  limits. 

In  Barzel  and  Barr’s  method,  they  use  the  parameter  r  to  control  the  speed  (not  computational 
speed)  by  which  the  constraint  is  to  be  met.  This  parameter  serves  as  a  weight.  So  to  maintain 
joints  as  point-to-point  constraints,  one  must  assign  very  small  r  to  those  constraints.  But  the  time 
step  of  the  computation  will  then  be  dominated  by  those  small  r. 

The  methods  of  both  Witkin  et  al  and  Barzel  et  al  solve  the  problem  in  6-t  space,  and  hence 
are  very  expensive  comparatively.  To  be  more  efficient,  one  often  chooses  adaptive  steps  for  time 
t.  But  the  step  of  t  is  mainly  decided  by  the  spatial  improvement.  So  if  we  are  given  a  spatial 
path  (in  Barzel  and  Barr’s  paper  [2],  the  deviation  function  actually  defines  a  spatial  path  in  terms 
of  time  t),  we  can  split  the  spatial  path  into  sufficiently  small  segments  and  get  a  smooth  spatial 
trajectory.  Moreover,  often  we  just  want  to  know  some  final  configurations  in  an  interactive  manner, 
such  as  making  a  human  figure  reach  somewhere,  look  at  something,  or  get  into  a  constrained 
environment.  In  human  or  robot  reach  space  analysis,  we  may  want  to  know  whether  or  not  some 
spatial  constraints  are  satisfiable  in  determining  the  reachable  space.  If  hundreds  of  reach  points 
are  involved,  the  speed  of  the  algorithm  is  very  crucial.  In  key  frame  animation,  we  create  some  key 
frames  and  let  intermediate  pictures  be  interpolated  either  using  function  interpolation  techniques 
[15]  or  motion  interpolation  techniques  using  optimal  control  theory  [3,  17].  In  many  situations, 
especially  in  human  animation,  joint  limits  are  very  important.  In  this  paper,  joint  limits  are  not 
special  cases  but  are  fundamental  to  the  procedure.  An  efficient  way  to  deal  with  joint  limits  during 
inverse  kinematic  positioning  is  an  important  concern. 

This  paper  is  devoted  to  solving  for  spatial  constraints  subject  to  joint  limits.  We  solve  the 
problem  in  6  space  (joint  angle  space  or  parameter  space).  It  is  very  fast.  For  example,  a  situation 
with  four  concurrent  goals,  involving  sixteen  degrees  of  freedom,  is  achieved  in  only  2.6  seconds 
(see  Figure  4). 

2  The  Method 

The  basic  geometric  entity  being  manipulated  is  the  articulated  figure.  The  data  structure  of  the 
articulated  figure  we  used  is  created  by  the  Peabody  language  developed  at  Computer  Graphics  Lab 
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at  University  of  Pennsylvania  [4].  A  Peabody  figure  is  composed  of  segments  connected  together 
by  joints  1 .  Each  joint  has  several  degrees  of  freedom  subject  to  joint  limits  and  users’  adjustment. 
Using  graph  terminology,  the  data  structure  of  the  Peabody  figure  is  a  tree,  where  segments  are 
nodes  and  joints  are  edges.  An  example  of  human  body  model  is  illustrated  in  Figure  1. 

A  spatial  constraint  involves  two  parts.  The  constraint  parts  on  the  figure  are  called  the  end 
effectors  and  their  counterparts  in  space  are  called  the  goals.  In  certain  contexts,  goals  and  con¬ 
straints  are  synonymous,  For  example,  a  position  goal  is  satisfied  means  that  a  position  constraint 
is  satisfied. 

Associated  with  each  goal,  there  is  a  non  negative  potential  function  P  such  that  it  is  zero  if 
and  only  if  the  goal  is  satisfied.  Since  we  are  only  concerned  about  the  spatial  constraint,  and  the 
spatial  position  and  orientation  are  determined  by  a  point  and  two  vectors  (a  coordinate  frame  has 
three  basis  vectors,  but  we  can  only  place  two  of  them  in  the  space),  the  potential  P  is,  in  general, 
the  function  of  a  position  and  two  unit  vectors,  say, 

P  =  P(r,  vi,  v2) 

where  r  is  the  position  vector,  and  vj,  v2  are  two  unit  vectors.  Of  course,  we  cannot  place  two  unit 
vectors  arbitrarily.  Their  angle  must  be  preserved.  We  call  it  the  potential  because  it  depends  only 
on  spatial  variables.  To  form  a  constraint,  we  just  plug  into  P  the  appropriate  variables  of  the  end 
effector  which  are  in  turn  the  functions  of  the  joint  angles,  i.  e.  , 

P(9)=  P(r(0W0),v2(0))  (1) 

where  0  is  the  vector  of  the  joint  angles.  Suppose  we  have  m  constraints,  then  the  overall  potential 
is  defined  as 

m 

P(9)  =  £  WiPi(9)  (2) 

i=i 

where  w,  are  weights  put  on  the  tth  constraint,  and  P,  is  the  potential  associated  with  the  ith 
constraint.  Clearly  all  the  constraints  are  satisfied  if  and  only  if  P  is  zero.  In  general,  constraints 
are  not  satisfied  simultaneously.  Our  task  is  to  minimize  the  potential  function  subject  to  joint 
limits.  In  most  cases,  joint  limits  are  described  by  linear  equalities  or  inequalities,  such  as  lower 

'Actually,  Peabody  figures  are  graph-structured  rather  than  being  limited  strictly  to  trees.  For  this  discussion, 
however,  the  tree  structure  of  the  human  or  robot  model  suffices. 
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Figure  1:  An  example  of  a  Peabody  human  figure  model 
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limits  and  upper  limits.  So  the  technique  of  nonlinear  programming  with  linear  constraints  is  used. 
Formally,  the  problem  is 

minP(0) 

'  s.t.  &J6  =  bi,i  =  1,2 . /  (3) 

af0<6„i  =  /+l,/  +  2,...,fc 

where  a,,  t  =  1, 2, . . . ,  k  are  rx-dimensional  column  vectors.  The  equality  constraints  allow  for  linear 
relations  among  the  joint  angles.  The  lower  limit  /;  and  upper  limit  u,-  on  0,  contribute  to  the  set 
of  inequality  constraints  on  9  as,  respectively: 

-0i  <  ~U 
9{  <  Ui 


The  algorithm  we  adopted  to  solve  this  problem  is  Davidon’s  variable  metric  method  with  the 
BFGS  (Broyden,  Fletcher,  Goldfarb,  Shanno)  approximate  inverse  Hessian  matrix  update  formula 
(to  know  its  merits,  see,  e.g.  [6,  9,  14])  and  Rosen’s  projection  method  to  handle  linear  constraints 
[13,  7].  Under  some  conditions,  the  method  is  superlinear  convergent  [12]  with  each  iteration  of 
complexity  of  0(n2  +  m)  where  n  is  the  total  number  of  joint  angles  involved,  and  m  is  the  number 
of  constraints.  The  method  is  robust  and  globally  convergent.  Of  course,  generally,  it  converges 
to  local  minima,  or  rather,  Kuhn-Tucker  points  (constrained  stationary  points),  rather  than  global 
minima. 

To  use  the  variable  metric  method,  we  need  to  compute  the  gradient  of  the  objective  function 
V0P.  From  the  definition, 

m 

VeP  =  ^w,VeP,  (4) 

i=i 


The  definition  of  VgP  is 


V*P  =  ( 


dP 


BP  BP_  \T 

357  '  ’ '  de„  ' 


where  n  is  the  dimension  of  the  vector  9. 

Usualiv,  the  number  of  joint  angles  involved  in  the  constraint  problem  n  is  less  than  the  total 
number  of  joint  angles  in  the  figure  we  are  dealing  with,  and  the  number  of  joint  angles  involved  in 
a  single  constraint  is  much  less  than  n.  So  it  is  both  computationally  economical  and  conceptually 
uniform  to  treat  individual  constraints  separately  and  then  assemble  them  together  to  get  V^P. 
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Figure  2:  Constraint  Chain 


3  Single  Constraints 


Since  our  data  structure  of  the  figure  is  like  a  tree  (see  Figure  1),  an  end  effector  of  a  constraint 
only  depends  on  those  joints  which  sit  along  the  path  from  the  root  of  the  figure  tree  to  the  segment 
where  the  end  effector  belongs  [1],  Let’s  call  this  path  the  constraint  chain.  The  constraint  chain  is 
illustrated  in  Figure  2,  where  a  joint  with  multiple  degrees  of  freedom  is  separated  conceptually  into 
several  joints  with  one  degree  of  freedom.  The  length  of  the  constraint  chain  is  the  total  number 
of  joints  (or  joint  angles)  along  the  chain.  In  Figure  2,  it  is  n.  But  do  not  confuse  this  n  with  that 
in  the  last  section.  We  are  only  dealing  with  one  constraint  in  this  section.  Because  all  the  joints 
of  the  human  body  are  revolute  joints,  we  discuss  here  only  revolute  joints.  But  the  translational 
joints  can  be  treated  similarly.  Let  the  ith  joint  angle  along  the  chain  be  0*,  the  axis  of  this  joint 
be  u  which  is  a  unit  vector,  the  position  vector  of  the  end  effector  be  r,  the  position  vector  of  the 
ith  joint  be  r,,  and  v  be  an  arbitrary  unit  vector  attached  to  the  end  effector  segment,  r  and  v  are 
functions  of  the  0,’s.  Actually,  they  can  be  computed  by  cascaded  multiplication  of  transformation 
matrices.  It  is  not  hard  to  see  that  (see  [18]) 


dr 

Wi 

dv 

dft 


u  x  (r  -  r.) 


u  x  v 


(5) 

(6) 


These  formulas  are  useful  in  deriving  the  gradient  of  the  potential  function. 

Let  the  potential  associated  with  this  constraint  be  P( r.  vj,V2),  and  g(9 )  be  VgP.  It  is  clear 


that 


9(0)  =  VeP 


<  w)Tv'F + ( ^L|TVv' f + <^,TVv>j 
'  if  r 


& 

v  ’■») 


VP 


(7) 


where  ||  is  a  3  by  n  matrix: 


(  3r_ 


9r 


) 


d&\  d§2 

and  similar  definition  for  and  ^j2-,  wh;~h  can  be  easily  computed  from  (5)  and  (C),  and  VrP, 
Vv,  P  and  VV3P  are  gradients  of  P  with  respect  to  r,  Vi  and  v2  respectively,  for  example, 


VrP  = 


dP 

5rv 


v* f  / 


where  rx  is  the  x  component  of  the  vector  r  and  similar  notation  for  y  and  z  components,  VP  is 
the  gradient  of  P  vrith  respect  to  spatial  variables,  or 


VP  = 


/  VtP  ^ 

VVlP 


\  Vv^  ) 

Notice  that  VP  is  independent  of  the  structure  of  the  articulated  figure. 

The  potential  P  can  be  very  simple,  as  will  be  seen  in  the  following,  but,  on  the  other  hand, 
it  could  be  very  computationally  expensive.  For  instance,  to  constrain  a  portion  of  the  figure  to 
some  region,  we  may  create  a  potential  function  such  that  it  is  zero  in  the  region  and  enough  big 
outside.  But  to  use  our  method,  we  need  the  gradient  of  the  function.  The  smooth  transition 
between  the  two  regions  often  requires  some  integral,  and  this  integral  usually  needs  comparatively 
costly  numerical  treatment.  Therefore,  in  evaluating  the  cost  to  compute  P(0)  and  g{0).  we  do  not 
count  the  cost  of  function  P  and  VP.  Under  this  convention  and  from  (5),  (6)  and  (7).  we  see  that 
g{6)  is  almost  as  expensive  as  P{6)  is.  which  is  dominated  by  n  multiplications  of  4  by  4  matrices, 
or  0{ n). 

Some  simple  but  useful  constraints  follow. 
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3.1  Position  Goal 


The  goal  is  a  point  in  the  3-dimensional  space.  Let  that  point  be  p,  and  the  end  effector  is  also 
a  point  which  sits  on  the  last  segment  of  the  constraint  chain.  Let  this  point  be  r  (see  Figure  2). 
The  potential  function  is: 

P  =  (  P-O2  (8) 


and  the  gradient  is: 


VrP  =  2(r  —  p) 


(9) 


VVlP  and  Vv2.P  are  zero. 


3.2  Orientation  Goal 

The  goal  is  an  orthonormal  coordinate  frame  in  space.  The  origin  of  the  frame  is  irrelevant.  Let 
the  goal  frame  be  : 

{p;x3,y3,z3} 

where  p  is  the  origin  and  x3,  y3,  z3  are  the  orthonormal  vectors.  Accordingly,  the  end  effector  is  an 
orthonormal  coordinate  frame  attached  at  f’.ie  last  segment  of  the  constraint  chain.  Let  this  frame 
be: 

{r;xe,ye,ze} 

The  potential  function  could  be: 

P  =  (x3  -  x..)2  +  (y3  -  ye)2 

But  this  function  implies  that  one  length  unit  is  as  important  as  about  one  radian  in  angle.  To 
enforce  one  length  unit  compatible  with  d  degrees  in  angle,  we  need  to  multiply  the  previous  P  by 
c,i  such  that 

—  -  —  d 
cd  ~  360  “ 

i.  e.  . 

q  =  360/(2 xrf)  (101 

To  be  more  general,  our  potential  function  is.  then. 

p  =  r2ix(x3  -  x-)2  +  cdy(yg  -  y.)2  (in 
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The  gradient  is: 


V^P  =  2  cL(x.-x#)  (12) 

Vy cp  =  24(ye  -  yg)  (13) 

Some  orientation,  say  y  direction,  could  be  suppresed  by  setting  Cdy  to  0.  This  is  useful,  for 
example,  to  constrain  a  person  holding  a  cup  of  water  to  keep  the  cup  upward  while  attaining  other 
constraints. 

3.3  Position/Orientation  Goals 

Position  and  orientation  goals  can  be  treated  separately,  but  sometimes  it  is  convenient  to  combine 
them  together  as  a  single  goal.  The  goal  and  end  effector  are  like  that  in  the  orientation  goal,  but 
the  origins  of  the  frames  are  important  here.  The  potential  function  for  position/orientation  goal 

is: 

P  -  wp(P  ~  r)2  +  v0clx(xg  ~  xe)2  +  w0Cdv{yg  -  ye)2  (14) 

where  xvp  and  wa  are  weights  put  on  position  and  orientation  respectively  such  that 

wp  +  w0-  1 

The  gradients  VrP, VXfP  and  Vy CP  are  obvious  from  Sections  3.1  and  3.2. 

3.4  Direction  Goals 

The  goal  is  a  point  in  space,  say,  point  p,  but  the  end  effector  is  a  vector  attached  to  the  end 
effector  segment.  Let  the  starting  point  of  that  vector  which  is  fixed  on  that  segment  be  r.  and  the 
vector  be  v  (see  Figure  2).  This  constraint  is  to  force  the  vector  v  to  point  toward  the  point  p. 
This  is  useful  when  we  want  to  make  a  body  look  in  some  direction  or  look  at  a  certain  point.  The 
potential  function  is: 

P  =  c](  rj~*~  r-  -  v)2  (15) 

Up  -  i’ll 

where  is  defined  in  (10)  and  ||  ■  ||  is  the  norm  of.  The  The  gradient  is: 

VTP  =  2cj(\\p  -  rj|2v  -  (p  -  r)-v(p  -  r))  (16) 


3.5  Line  Goals 


The  goal  is  a  line  and  the  end  effector  is  a  point  r.  This  constraint  forces  the  point  to  go  to  the 
line.  Let  the  line  be  defined  by  point  p  and  a  unit  vector  u  such  that  the  parametric  equation  of 
the  line  is 

p  +  tv 


The  potential  function  is: 


P  =  ((  p-r)  — (p-r)-t/i/)2 


(18) 


and  the  gradient  is: 


VrP  =  2(i/-(p  -  r)  v  -  (p  -  r)) 


(19) 


3.6  Plane  Goals 

The  goal  is  a  plane  and  the  end  effector  is  a  point  r.  This  constraint  forces  the  point  to  go  to  the 
plane.  Let  a  point  on  the  plane  be  p  and  the  normal  of  the  plane  be  v. 

The  potential  function  is: 

P  —  ((P  ~  r)-i/)2  (20) 

and  the  gradient  is: 

VrP  =  -  r)  v  (21) 


4  Assembly  of  Local  Gradient  to  Global  Gradient 

We  have  dealt  with  various  constraints  in  Section  3.  Of  course,  the  types  of  constraints  are  not 
limited  in  the  Section  3;  they  are  only  some  examples.  The  problem  now  is  to  assemble  all  the 
information  about  individual  constraints  to  form  one  constraint. 

Suppose  we  have  m  constraints.  The  ith  constraint  has  constraint  chain  of  length  n,  and  the 
joint  angles 

©*  =  . (22) 

Since  constraint  chains  are  from  a  single  figure  tree.  0'’s  may  overlap  with  each  other.  Let 

0  =  0©' 

1=1 

=  {01,02 . 0n}  (23) 
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The  global  index  of  9  has  nothing  to  do  with  the  topological  relation  within  joint  angles,  but  the 
local  index  does.  They  are  numbered  from  the  starting  point  of  the  constrain'  chains  to  the  end 
effectors.  For  each  constraint,  we  have  a  mapping  from  local  index  to  global  index: 


M‘  :  {1,2,. *{l,2,...,n}  (24) 

such  that  9 )  is  9M,^  in  the  global  index.  It  is  easy  to  compute  P(9)  from  P,  from  Equation  2. 
We  just  need  to  take  care  of  g{9)  —  V$P.  The  local  gradient  for  each  constraint  can  be  obtained 
from  Section  3.  Notice  that  in  that  section  the  derivatives  are  with  respect  to  local  0,’s  contrary  to 
global  9  in  (4).  g(9)  in  (7)  is  a  n,-dimensional  vector  while  V&P  in  (4)  is  a  n  dimensional  vector. 
Let 

9'  =  (  9\  92  ■■■  9ln,  )T 

be  the  local  gradient  of  the  ith  constraint,  and 

9  =  (  9i  92  9n  )T 

be  the  global  gradient.  We  can  simply  assemble  g  from  g" s  as  follows: 

Step  1.  gj  <—  0,  for  j  =  1,2, . . . ,  n 

Step  2.  For  t  =  1  to  m  do 

9m>U)  *-  9M'(j)  +  Wig),  for  j  =  1,2, ... ,  n, 

4.1  The  Algorithm  for  Nonlinear  Programming  Problem 

We  are  now  ready  to  solve  the  problem  (3).  From  Section  3  and  Section  4,  we  can  very  effectively 
compute  P(9)  and  g(9 )  =  V$P(0)  in  0{n  +  m).  There  are  many  algorithms  available  to  solve  the 
problem.  Among  them,  the  variable  metric  method  (or  conjugate  gradient  method)  is  considered 
most  powerful  for  unconstrained  problems  with  a  smooth  objective  function.  Rosen’s  projection 
method  is  very  effective  in  treating  linear  constraints  [13].  Goldfarb  combined  DFP’s  method  (a 
variable  metric  algorithm)  [5]  with  Rosen's  projection  method  [7].  But  after  t hat.  the  variable 
metric  method  was  much  improved.  BFGS’  improvement  has  been  considered  most  effective.  One 
of  the  motives  of  the  improvement  is  to  try  to  get  best  conditioning  of  the  approximate  inverse 
Hessian  matrix  [14].  The  algorithm  we  are  presenting  here  is  the  combination  of  the  BFGS  method 
and  Rosen's  projection  method.  We  follow  very  closely  Goldfarb's  paper  [7], 
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Without  loss  of  generality,  we  assume  that  all  the  a,’s  in  (3)  are  unit  vectors.  We  say  that  point 
9  is  feasible  if  it  satisfies  all  the  equalities  and  inequalities  in  (3).  The  ith  constraint  is  said  to  be 
active  at  9  if  a.J  9  =  So  an  equality  constraint  is  always  active  at  a  feasible  point.  We  assume 
further  that  at  any  point,  the  a,’s  for  active  constraints  are  linearly  independent.  Let  Aq  denote  a 
n  by  q  matrix  derived  from  lumping  together  q  vectors  from  a,’s,  i.e.  , 


A,  =  (  a^,  a,-,  ) 

In  the  following  description  of  the  algorithm,  the  superscript  i  denotes  the  ith  iteration.  The 
algorithm  follows. 

Step  0.  Let  9°  be  a  initial  feasible  point,  and  H °  a  initially  chosen  n  by  n  positive  definite  sym¬ 
metric  matrix.  Suppose  there  are  q  constraints  active  at  point  9°.  Aq  is  composed  of  these 
a,’s  and  first  l  columns  of  Aq  are  {a,-  :  i  =  1,2,...,/}.  Hq  is  computed  by  employing  (27)  q 
times.  g°  =  g(9°). 


Step  1.  Given  9',g‘  and  Hq ,  compute  Hqg'  and 

a  =  (AjAq)-lA*g' 


If  Hjg1  =  0  and  aj  <  0,  j  =  /  +  1,  /  +  2, . . . ,  q,  then  stop.  9'  is  a  Kuhn-Tucker  point. 


Step  2.  If  the  algorithm  did  not  terminate  at  Step  1,  either  >  max{0,  jaqaqq^}  or 

\\H'qgl\\  <  j aqaqq^2,  where  it  is  assumed  that  aqaqq^2  >  a.a^1^2 ,j  =  /  +  —  1  and 

where  an  is  the  *th  diagonal  element  of  (A^A7)-1.  (They  are  all  positive,  see  [7]) 


If  the  former  holds,  proceed  to  Step  3. 

Otherwise,  drop  the  q th  constraint  from  A,  and  obtain  Hq_l  from 


K-x  =  H'  + 


Pq— lai<,az„  ^9  —  1 


(25) 


where  F7_i  =  /- A,_i(  A^.,  A7_i)  1A^’_1  is  a  projection  matrix,  a, q  is  the  7th  column  of  A,, 
and  A7_i  is  the  n  by  q  -  1  matrix  got  from  taking  off  the  c/th  column  from  .  l7. 

Let  q  —  q  —  \  and  goto  Step  1. 
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Step  3.  Let  the  search  direction  s‘  =  — H'g ‘  and  compute 


Xj  =  - j—i — ,j  =  q+l,q+2,...,k 

a j  s' 

A'  =  min{Aj  >  0} 


Using  any  line  search  technique  to  obtain  biggest  possible  7'  such  that  0  <  7*  <  min{l,A‘}, 
and 

'  P{9'  +  7W)  <  P(6')  +  6l7'(gi)Tsi 

g(9'  +  7's')ts'  >  S2(gi)Ts> 

where  £1  and  62  are  positive  numbers  such  that  0  <  <  62  <  1  and  <5j  <  0.5.  Let  d‘+l  = 

6 *  +  7 V  and  p,+1  =  p(0,+1). 


Step  4.  If  7*  =  A‘,  add  to  Aq  the  a y  corresponding  to  the  min{Ay}  in  Step  3.  Then  compute 

ffi+ 1  _  rri 

q+l  ~H'~  &J  H ^  a. j 

Set  q  <—  q  +  1  and  i  «—  t  +  1  and  goto  Step  1. 

Step  5.  Otherwise,  set  o'  -  7 ‘s*  and  y'  =  p,+1  -  g'  and  update  H'q  as  follows: 

If  {a')Ty'  >  (y')TH'qy'  then  use  the  BFGS  formula: 

^•+1  =  **  +  ((!  +  ^rMa')r  -  oWfn*  -  H'qy'{a')T)/(<7')Ty' 


(27) 


else  use  the  DFP  formula: 


H»x  =  gji  ,  W)7  _  H'qy'(y')TH'q 

q  q  (<r')Ty'  {y')THqy' 


(28) 


(29) 


Set  «*—*  +  !  and  goto  Step  1. 


The  inexact  line  search  strategy  (26)  in  Step  3  was  proposed  by  Powell  [11]  and  61  =  0.0001  and 
S2  =  0.5  were  suggested  in  [11].  Since  s'  is  a  descent  direction,  i.  e.  .  (g')Ts‘  <  0.  this  strategy 
guarantees  that  the  function  value  is  decreased  and  {o')Tyl  >  (1  —  <’>2)|(fl,,)T'S'|  >  0.  Because,  as 
we  pointed  out  in  Section  3,  the  gradient  g(0)  is  almost  as  expensive  as  the  function  P{9).  we  used 
cubic  Hermite  interpolation  method  in  the  line  search.  We  feel  it  is  very  effective. 

The  switch  between  the  BFGS  formula  and  the  DFP  formula  was  suggested  by  Fletcher  [6]. 
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Notice  that  all  matrix  multiplications  are  performed  as  a  n  by  n  matrix  and  a  vector,  or  a  n 
by  1  matrix  and  a  1  by  n  matrix.  For  example,  matrix  multiplication  HqajajHq  can  be  grouped 
as  (Hqa:)(H'qa})T .  The  inverse  of  a  matrix  might  take  much  time,  but,  fortunately,  for 
we  have  a  very  effective  recursive  relation  of  (AjAq)~l  to  (A^+lAq+i)~x  and  (A^^A^i  )-1  (see 
[7]  for  details).  So  the  complexity  of  one  iteration  is  0(n2). 

The  correctness  of  the  algorithm  was  proved  by  Goldfarb  in  [7]  for  exact  line  search  in  Step  3 
and  the  DFP  formula  in  Step  5.  But  it  is  not  hard  to  follow  the  proof  in  [7]  to  show  the  correctness 
of  our  algorithm.  Be  careful  that  [7]  was  for  maximum  while  our  algorithm  is  for  minimum.  We 
tried  both  the  BFGS  and  DFP  formula  and  found  that  BFGS  is  really  better.  Shanno  compared 
them  in  [14]  for  many  functions,  and  the  results  are  generally  in  favor  of  the  BFGS  formula.. 

5  Some  Remarks 

•  We  assumed  in  Section  3  that  the  constraint  chain  went  from  the  root  of  the  figure  tree  to 
some  end  effector.  It  is  possible  and  sometimes  useful  that  the  chain  goes  from  a  specified 
joint  which  is  nearer  to  the  root  than  the  end  effector  is.  But  then  we  must  take  care  of 
those  joints  which  are  not  in  this  constraint  chain  but  are  in  another  chain  and  affect  this 
end  effector.  We  must  add  those  joints  to  this  chain. 

•  Suppose  some  joints  are  active  and  some  joints  are  inactive,  we  can  add  joints  to  the  constraint 
chain  dynamically  according  to  their  activeness. 

•  The  obstacle  avoidance  problem  can  also  be  treated  here.  But  we  do  not  look  for  a  path 
which  does  not  touch  the  obstacle.  Instead,  we  are  concerned  about  those  parts  which  are 
pulled  by  the  end  effector,  since,  usually,  the  end  effector  is  assigned  a  goal  which  does  not 
intersect  with  the  obstacle.  For  example,  we  may  want  the  hand  to  get  to  some  place  while 
keeping  the  elbow  away  from  the  obstacle.  We  can  create  a  potential  function  around  the 
obstacle  and  assign  this  goal  to  the  elbow. 
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Figure  3:  Standing  Body 


6  Implementation 


We  have  implemented  the  multiple  goal  positioning  in  the  Jack  interactive  environment  [4].  By 
positioning,  we  mean  to  satisfy  spatial  constraints.  It  is  fast  enough  to  be  used  in  an  interactive  en¬ 
vironment.  It  has  been  used  for  simple  positioning,  or  for  creating  key  frames  for  later  interpolation 
by  spline  functions  or  by  dynamical  simulations. 

The  examples  given  here  were  run  using  Jack  on  a  Silicon  Graphics  IRIS  4D/70GT.  Figure  3  is 
an  initial  configuration.  From  that  position,  we  use  4  position  goals,  2  for  elbows  and  2  for  hands, 
to  get  the  posture  as  in  Figure  4.  Two  constraint  chains  are  from  shoulders  to  hands,  and  another 
two  from  shoulders  to  elbows.  It  involves  16  degrees  of  freedom  and  takes  2.6  seconds. 

Figure  5  has  two  goals  for  two  hands.  The  goals  are  on  the  bar  which  are  not  reachable.  It 
involves  31  degrees  of  freedoms  and  runs  in  13  seconds. 

Figure  6  is  a  person  holding  a  box.  To  deliver  the  box  to  the  goal  shown  on  the  figure,  we  used 
a  position/orientation  goal  to  keep  the  box  from  tipping  upside  down.  Position  and  orientation 
have  the  same  weight.  5  degrees  of  angle  is  made  as  important  as  1  unit  of  length.  The  result  is  in 
Figure  7.  It  involves  10  degrees  of  freedom  and  takes  2  seconds. 

In  conclusion,  this  multiple  goal  achievement  algorithm  is  a  significant  improvement  over  other 
inverse  kinematic  procedures  based  on  its  generality,  speed,  and  fundamental  use  of  joint  limits  and 
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Figure  4:  Four  position  goals:  2.6  seconds 


Figure  5:  Bending  over  a  bar:  13  seconds 
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Figure  7:  To  deliver  a  box:  2  seconds 
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spatial  constraints.  It  is  a  major  convenience  in  the  interactive  manipulation  of  articulated  figures 
for  positioning,  reaching,  and  viewing  tasks. 
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